function Milisg_2012_11_30
clear all;
clc;
format long;
warning off all
clf(figure(1))

javaaddpath 'C:\Users\user\Documents\MILISG\My Dropbox\Software\PhD-SoftwareSolutions\BACnet\bacnet4J.jar';
javaaddpath 'C:\Users\user\Documents\MILISG\My Dropbox\Software\PhD-SoftwareSolutions\BACnet\seroUtils.jar';
javaaddpath 'C:\Users\user\Documents\MILISG\My Dropbox\PhD\Research\1.CodeBase\JavaCode\BACnetNetwork1\build\classes'

import java.lang.*;
import com.serotonin.bacnet4j.*; 
import com.serotonin.bacnet4j.apdu.*;  
import com.serotonin.bacnet4j.base.*; 
import com.serotonin.bacnet4j.enums.*;  
import com.serotonin.bacnet4j.event.*;  
import com.serotonin.bacnet4j.exception.*;  
import com.serotonin.bacnet4j.npdu.*;  
import com.serotonin.bacnet4j.npdu.ip.*;  
import com.serotonin.bacnet4j.obj.*;  
import com.serotonin.bacnet4j.service.*;  
import com.serotonin.bacnet4j.service.acknowledgement.*;  
import com.serotonin.bacnet4j.service.confirmed.*;  
import com.serotonin.bacnet4j.service.unconfirmed.*;  
import com.serotonin.bacnet4j.test.*;  
import com.serotonin.bacnet4j.type.*;  
import com.serotonin.bacnet4j.type.constructed.*;  
import com.serotonin.bacnet4j.type.enumerated.*;  
import com.serotonin.bacnet4j.type.error.*;  
import com.serotonin.bacnet4j.type.eventParameter.*;  
import com.serotonin.bacnet4j.type.notificationParameters.*;  
import com.serotonin.bacnet4j.type.primitive.*;  
import com.serotonin.bacnet4j.util.*;  
import bacnetnetwork1.*;

%parameters of plant (three-rooms system)
n=3; % number of states

vAir=0.104; % m/s
c1=10.45 - vAir+10*sqrt(vAir); %heat transfer coefficient of air (W/m^2.K) - c = 10.45 ? v + 10?v , with normal v air = 0.104 m/s. For v=20 m/s, the coefficient becomes 35.17
c2=0.5/0.2; % heat transfer coefficient of brick =thermal conductivity (e.g. 70 W/m.C) / thickness (e.g. (50 mm) - (W/m^2.C^0). %we assume walls of thickness 20cm=0.2m and material brick of 3Kg with thermal
%conductivity 0.5
ro=1.29; %density of air (Kg/m3)
cp=1005; %specific heat capacity of air (J/kg.K)
Adoor=2.0; % Area of door (m^2)
Awindow=0.5; % Area of window (m^2)
Awall=9; % Floor and roof area = 3x3
%Awe=3*2.5; % Area of east-west walls
%Asn=4*2.5; % Area of north-south walls
V=3*3*3; % volume of room (m3)
H=cp*ro*V; % Heat capacity of air in room (J/C)
uMax=2000;
stepSize=100;
uSteps=[-uMax:stepSize:uMax]; % steps of inflow heat transfer by electric heater (W)
TaMax=22; %oC
TaMin=8; %oC
TaMag=TaMax-TaMin; % magnidute of air temperature variations
TaMean=(TaMax+TaMin)/2; % mean value of air temperature in a day

%x_dot=[-Cf*4*Aw -Cf*Aw 0;Cf*Aw -Cf*Aw-Cf*4*Aw-Cf*Aw +Cf*Aw;0 Cf*Aw -Cf*Aw-Cf*4*Aw]*[x1 x2 x3]'+[1/H 0 0]*U + [+Cf*4*Aw*Ta +Cf*3*Aw +Cf*4*Aw];
A=1/H.*[-c1*Adoor c1*Adoor 0;c1*Adoor -c1*Adoor-c1*Adoor c1*Adoor;0 c1*Adoor -c1*Adoor];
B=1/H.*[1; 0; 0];
%D=1/H.*[4*c2*Awall+c1*Awindow; 3*c2*Awall+c1*Awindow; 4*c2*Awall+c1*Awindow];%*Ta
C=[1 0 0;0 1 0;1 0 0];

%Definition of scenarios for execution - %other initialization and
%configuration parameters

r=25*ones(n,1); %desired/reference temperature for rooms in Celsius
superTitle={'Yes changes - Yes mediation'};
simulationTime=50;
simulateHourTime=60;
Ts=5/60; % sampling time 5 minutes
pricePerKwh=0.5; % euro per KWh
k=1; %discrete step counter

%%%%%%%%%%%%%%%%%%% BACnet-Network %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bacnetNetworkPort=2107;
tmp = input(sprintf('Enter the BACnet network port number (e.g. %4.0f):\n', bacnetNetworkPort));
if ~isempty(tmp)
    bacnetNetworkPort = tmp;
end

bacnetNetworkAddress='127.0.0.255';
bacnetNetworkNumber=1;
bacnetNetwork=BacNetNetwork(bacnetNetworkNumber,bacnetNetworkAddress);

% Sensors
numberOfSensors=7;

for sensorCounter=1:numberOfSensors    
    sensorAddress{sensorCounter}={['127.0.0.' int2str(sensorCounter)]};    
end

sensor(1)=Sensor(1, bacnetNetworkAddress, sensorAddress{1}, bacnetNetworkPort, 'Sensor 1', EngineeringUnits.degreesCelsius);
sensorAnnotation(1)=SensorAnnotation(1,'temperature', 'celsius', 5, 'Room 1', 0);
sensor(1).setOutOfService(0);
sensor(2)=Sensor(2, bacnetNetworkAddress, sensorAddress{2}, bacnetNetworkPort, 'Sensor 2', EngineeringUnits.degreesCelsius);
sensorAnnotation(2)=SensorAnnotation(2,'temperature', 'celsius', 5, 'Room 2', 0);
sensor(2).setOutOfService(0);
sensor(3)=Sensor(3, bacnetNetworkAddress, sensorAddress{3}, bacnetNetworkPort, 'Sensor 3', EngineeringUnits.degreesCelsius);
sensorAnnotation(3)=SensorAnnotation(3,'temperature', 'celsius', 5, 'Room 3', 0);
sensor(3).setOutOfService(0);
sensor(4)=Sensor(4, bacnetNetworkAddress, sensorAddress{4}, bacnetNetworkPort, 'Sensor 4', EngineeringUnits.degreesFahrenheit);
sensorAnnotation(4)=SensorAnnotation(4,'temperature', 'fahrenheit', 2, 'Room 3', 0);
sensor(4).setOutOfService(1);
sensor(5)=Sensor(5, bacnetNetworkAddress, sensorAddress{5}, bacnetNetworkPort, 'Sensor 11', EngineeringUnits.degreesCelsius);
sensorAnnotation(5)=SensorAnnotation(5,'temperature', 'celsius', 1, 'Room 2', 0);
sensor(5).setOutOfService(1);
sensor(6)=Sensor(6, bacnetNetworkAddress, sensorAddress{6}, bacnetNetworkPort, 'Sensor 12', EngineeringUnits.degreesCelsius);
sensorAnnotation(6)=SensorAnnotation(6,'temperature', 'celsius', 2, 'Room 3', 0);
sensor(6).setOutOfService(1);
sensor(7)=Sensor(7, bacnetNetworkAddress, sensorAddress{7}, bacnetNetworkPort, 'Sensor 21', EngineeringUnits.degreesCelsius);
sensorAnnotation(7)=SensorAnnotation(7,'temperature', 'celsius', 2, 'Ambient', 0);
sensor(7).setOutOfService(1);

% Actuators - ElectricRadiators
eRadiatorAddress{1}={'127.0.0.101'};
eRadiator(1)=ElectricRadiator(101, bacnetNetworkAddress, eRadiatorAddress{1}, bacnetNetworkPort, 'Elecrtic Radiator 1', EngineeringUnits.watts);

% Controller
controllerAddress{1}={'127.0.0.201'};
outputsNumber(1)=1;
refInputsNumber(1)=3;
controlParametersNumber(1)=1;
controller(1)=Controller(201, bacnetNetworkAddress, controllerAddress{1}, bacnetNetworkPort, 'Controller 1', EngineeringUnits.watts, outputsNumber(1), refInputsNumber(1), controlParametersNumber(1));

% SSM device
ssmAddress{1}={'127.0.0.202'};
ssmOutputsNumber(1)=3;
ssm(1)=SSM(202, bacnetNetworkAddress, ssmAddress{1}, bacnetNetworkPort, 'SSM 1', ssmOutputsNumber(1));

communicate=Communicate();

%controlled system parameters + controller design parameters
%design of controller
Poles=4*eye(n);
[K L]=findDesignParametersController(A,B,Poles);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%definition of time-related information
tstart = 1;
tend = simulationTime*simulateHourTime;
steps = 1; % steps of 1 minute
timespan=(tstart:steps:tend);
thours=timespan./simulateHourTime;

%modelling of outside temperature, as well as, bounds for outside
%temperature and Room 2 temperature
Ta=modelOutsideTemperature(thours, TaMean, TaMag);
[TaUpperBound TaLowerBound] = modelBoundsOutsideTemperature(Ta);
[T2UpperBound T2LowerBound] = modelBoundsRoom2Temperature(Ta);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Beginning of scenario
for iter=tstart:steps:tend    
    currentTime=thours(iter);

    %%%%%%%%%%%%%%%%%%%Implement scenarios of sensors%%%%%%%%%%%%%%%
    if (currentTime >= 1)
        sensor(5).setOutOfService(0);
    elseif (currentTime >= 2)
        sensor(5).setOutOfService(1);
    elseif (currentTime >= 5)
        sensor(3).setOutOfService(1);
    elseif (currentTime >= 6)
        sensor(6).setOutOfService(0);
    elseif (currentTime >= 7)
        sensor(6).setOutOfService(1);
        sensor(7).setOutOfService(0);
    elseif (currentTime >= 17)
        sensorAnnotation(5).setLocation('Room 3');
        sensor(5).setOutOfService(0);
    elseif (currentTime >= 19)
        sensor(5).setOutOfService(1);
    elseif (currentTime >= 22)
        sensor(7).setOutOfService(1);
    elseif (currentTime >= 29)
        sensor(4).setOutOfService(0);
    end 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % initializations during first step of scenario
    if (iter==1)
        %initialization of dynamic variables and integration/simulation
        xIni=[18 20 22]';
        sensor(1).setSensorValue(xIni(1)); 
        sensor(2).setSensorValue(xIni(2));
        sensor(3).setSensorValue(xIni(3));
        ssm(1).setOutputValue(1, xIni(1));
        ssm(1).setOutputValue(2, xIni(2));
        ssm(1).setOutputValue(3, xIni(3));
        
        for i=1:n
            prId=PropertyIdentifier.presentValue;            
            objId=controller(1).getObject(['Reference value ', int2str(i)]).getId();
            communicate.communicateWriteReal(ssm(1),controllerAddress{1},bacnetNetworkPort,objId, prId, r(i));
        end
        uIni=produceControllerOutput(n, communicate,controller(1), ssm(1), ssmAddress{1}, eRadiator(1), eRadiatorAddress{1}, bacnetNetworkPort, uSteps, K, L);
        
        %initialization of progress bar
        h = waitbar(0,'Initializing waitbar...','Name','Progress...');
        stepsint=100;
        stateIni=[xIni' uIni];
        MassMatrix=diag([ones(1,n) ones(1,1)]);
        options =odeset('Mass',MassMatrix,'Reltol',0.1,'AbsTol',0.1,'Stats','off','MassSingular','no');            
        
        yfinal=C*xIni;
        PD = fitdist(xIni, 'normal');
        TaCurrent=TaMean;
        uref=uIni;        
    else           
        %tasks at subsequent steps of scenario    
        
        % upper and lower bounds of system disturbance
        %dl1=A(1,2)*T2LowerBound(iter);
        %dl2=D(1)*TaLowerBound(iter);
        %dl=dl1+dl2;
        %du1=A(1,2)*T2UpperBound(iter);
        %du2=D(1)*TaUpperBound(iter);
        %du=du1+du2;
        
        stateIni=[X(:,iter-1)' U(iter-1)];        
    end
    
    %parameters for integration
    tspan = linspace(iter-1,iter,stepsint);
    varargin={n,A,B,uref};
    [junk_t,solution] = ode45(@myodefun, tspan, stateIni,options,varargin);

    % processing of integration outcome
    [s junk]=size(junk_t);
    X(:,iter)=(solution(s,1:n))';
    U(iter)=(solution(s,n+1))';
    
    % dream measurement
    y=C*X(:,iter);
 
    % recording of measurements from available sensors
    for sensorCounter=1:numberOfSensors        
        recordMeasurement(y,sensor(sensorCounter),sensorAnnotation(sensorCounter),currentTime);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %retrieval of measurements and processing to produce the control
    %feedback vector    
    samplingTime=k*Ts;
    if (currentTime >= samplingTime)
        for ii=1:n+1
            y2(ii,1)=0;
            y2(ii,2)=0;
        end
        for sensorCounter=1:numberOfSensors            
            [value(sensorCounter) loc(sensorCounter) when(sensorCounter) accur(sensorCounter)]=retrieveMeasurement(communicate,sensor(sensorCounter),sensorAnnotation(sensorCounter),sensorAddress{sensorCounter},ssm(1),bacnetNetworkPort);        
            totalWeight(sensorCounter)=weightTime(currentTime, when(sensorCounter))*weightAccuracy(accur(sensorCounter));
            y2(loc(sensorCounter),1)=y2(loc(sensorCounter),1)+value(sensorCounter)*totalWeight(sensorCounter);
            y2(loc(sensorCounter),2)=y2(loc(sensorCounter),2)+totalWeight(sensorCounter);
        end        
               
        for ii=1:n+1            
            yfinal(ii)=y2(ii,1)/y2(ii,2);
        end
        
        ssm(1).getObject('SSM output 1').setProperty(PropertyIdentifier.presentValue, Real(yfinal(1)));
        
        uref=produceControllerOutput(n, communicate,controller(1), ssm(1), ssmAddress{1}, eRadiator(1), eRadiatorAddress{1}, bacnetNetworkPort,uSteps,K,L);
        
        k=k+1;
    end        
    
    %calling of progress bar
    progressBar(thours(iter),simulationTime,h);
end %end of iterations of scenario    
    
%closing progress bar
close(h); 
%stop devices
for sensorCounter=1:numberOfSensors
    sensor(sensorCounter).terminate();
end

eRadiator(1).terminate();
controller(1).terminate();
ssm(1).terminate();
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%calculation of total cost for heating
EconomicCost=calcEconomicCost(pricePerKwh, U,timespan)
FunctionalCost=calcFunctionalCost(r, X)    
TotalCost=0.3*EconomicCost+0.7*FunctionalCost
%%%%%%%%%%%%%%%%%%%

%plots of solution and other outputs
f=figure(1);

fontname='Times-Roman';
fontsize=12;

subplot(2,1,1);
h1=plot(thours,X(1,:),'-r',thours,X(2,:),'-b',thours,X(3,:),'-g');
set(gca,'FontSize',fontsize,'FontName',fontname);
grid on;
axis([0 60 0 30]);
title1=title('System State Vs Time');
xlabel1=xlabel('Time (Hours)');
ylabel1=ylabel('Temperature (Fahrenheit)');
leg1=legend('$T_1$', '$T_2$','$T_3$');
set(h1,'LineWidth',2)
set(leg1,'interpreter','latex','FontSize',fontsize,'FontName',fontname,'Layer','top');
set(title1,'FontSize',fontsize,'FontName',fontname,'FontWeight','bold');
set(xlabel1,'FontSize',fontsize,'FontName',fontname);
set(ylabel1,'FontSize',fontsize,'FontName',fontname);

%Control input Vs Time
subplot(2,1,2);
h1=plot(thours,U(:),'-r');
set(gca,'FontSize',fontsize,'FontName',fontname);
grid on;
axis([0 60 -2500 2500]);
title3=title('Control input Vs Time');
xlabel3=xlabel('Time (Hours)');
ylabel3=ylabel('Control input (Watts)');
leg3=legend('$u_1$');
set(h1,'LineWidth',2);
set(leg3,'interpreter','latex','FontSize',fontsize,'FontName',fontname,'Layer','top'); 
set(title3,'FontSize',fontsize,'FontName',fontname,'FontWeight','bold');
set(xlabel3,'FontSize',fontsize,'FontName',fontname);
set(ylabel3,'FontSize',fontsize,'FontName',fontname);  

tbh = annotation(f,'textbox',[0.02 0.96 0.9 0.04]);
titletextw = textwrap(superTitle,180);
set(tbh,'EdgeColor','none','Interpreter','none','string',titletextw,'FontSize',20,'FontWeight','bold','HorizontalAlignment','center');   

%figure(2);
%plot(thours,AdaptBounds(1,:),thours,AdaptBounds(2,:),thours,AdaptBounds(3,:),thours,AdaptBounds(4,:))
%grid;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function performing the integration of the differential equations, given
% the current state.
function [dsdt] = myodefun(t,state,varargin)

n=varargin{1}{1};
A=varargin{1}{2};
B=varargin{1}{3};
uref=varargin{1}{4};

%logging of the states
x=state(1:n);
u=state(n+1);

xdot=A*x+B*u;
udot=-8*u+8*uref;

dsdt=zeros(size(state));
dsdt(1:n)=xdot; 
dsdt(n+1)=udot; 

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f=SEMfunction1(x)
    f=(x-32)/1.8;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function progressBar(thours,simulationTime,h)
 
perc = thours*100/simulationTime;
waitbar(perc/100,h,sprintf('%12.4g%% along...',perc));        
end
% --------------------------------------------------------------------------
function EconomicCost = calcEconomicCost(pricePerKwh, U, timespan)
[m s2]=size(U);
for i=1:m
    cost(i)=(abs(U(i,1))/1000)*timespan(1)*pricePerKwh;
    for j=2:s2
        cost(i)=cost(i)+(abs(U(i,j))/1000)*(timespan(j)-timespan(j-1))*pricePerKwh;
    end        
end

EconomicCost=0;
for i=1:m
    EconomicCost=EconomicCost+cost(i);
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function FunctionalCost = calcFunctionalCost(r, X)
[n s]=size(X);
for i=1:s
    Xr(:,i)=(X(:,i)-r).^2;
end

for i=1:n
    cost(i)=0;
    for j=1:s
        cost(i)=cost(i)+Xr(i,j);
    end        
end
FunctionalCost=0;
for i=1:n
    FunctionalCost=FunctionalCost+cost(i);
end

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function uProcessed = processControlInput(u, uSteps)
    tmp = abs(u-uSteps);
    [idx idx] = min(tmp); %index of closest value
    closest = uSteps(idx); %closest value
    uProcessed=closest;    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Ta = modelOutsideTemperature(thours,TaMean,TaMag)
    for ii=1:length(thours)
        tcurrent=mod(thours(ii),24);
        if (tcurrent==0)
            tcurrent=24;
        end
        omega=2*pi*(tcurrent-1)/24;
        Ta(ii)=TaMean+TaMag*(0.4632*cos(omega-3.805)+0.0984*cos(2*omega-0.360)+0.0168*cos(3*omega-0.822)+0.0138*cos(4*omega-3.513));   
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [TaUpperBound TaLowerBound] = modelBoundsOutsideTemperature(Ta)
    for ii=1:length(Ta)
        TaUpperBound(ii)=Ta(ii)+2;
        TaLowerBound(ii)=Ta(ii)-2;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [T2UpperBound T2LowerBound] = modelBoundsRoom2Temperature(Ta)
    for ii=1:length(Ta)
        T2UpperBound(ii)=Ta(ii)+4;
        T2LowerBound(ii)=Ta(ii);
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function controllerOutput=produceControllerOutput(n, communicate,controller,ssm,ssmAddress, eRadiator, eRadiatorAddress,bacnetNetworkPort, uSteps, K, L)
    import com.serotonin.bacnet4j.type.enumerated.*;      
    for i=1:n
        objId=ssm.getObject(['SSM output ',int2str(i)]).getId();
        prId=PropertyIdentifier.presentValue;
        measurement(i)=communicate.communicateRead(controller,ssmAddress,bacnetNetworkPort,objId, prId).floatValue();
        reference(i)=controller.getReferenceValue(i).floatValue();
    end
    
    error=(measurement-reference);
    
    controllerOutput=processControlInput(K*error'+L*reference',uSteps);
    controller.setControllerOutput(1,controllerOutput);    
    objId=eRadiator.getObject('Actuator output').getId();
    prId=PropertyIdentifier.presentValue;
    communicate.communicateWriteReal(controller,eRadiatorAddress,bacnetNetworkPort,objId, prId, controllerOutput);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function loc=where(location)
    if strcmp(location,'Room 1')
            loc=1;
    elseif strcmp(location,'Room 2')
            loc=2;
    elseif strcmp(location,'Room 3')
            loc=3;
    elseif strcmp(location,'Ambient')
            loc=4;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function recordMeasurement(y,sensor,sensorAnnotation,currentTime)
    sensorOutOfSercice=sensor.getOutOfService();
    if (sensorOutOfSercice=='false')     
        aa=y(where(sensorAnnotation.getLocation()))*(1-sensorAnnotation.getAccuracy()/100);
        bb=y(where(sensorAnnotation.getLocation()))*(1+sensorAnnotation.getAccuracy()/100);
        tempValue=aa + (bb-aa)*rand(1,1);
        if strcmp(sensorAnnotation.getUnits(),'celsius')
            sensor.setSensorValue(tempValue);
        elseif strcmp(sensorAnnotation.getUnits(),'fahrenheit')
            sensor.setSensorValue(1.8*tempValue+32);
        end
        sensorAnnotation.setLastMeasurementTime(currentTime);
    end    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [value loc when accur] = retrieveMeasurement(communicate,sensor,sensorAnnotation,sensorAddress,ssm, bacnetNetworkPort)
    import com.serotonin.bacnet4j.type.enumerated.*;      
    objId=sensor.getObject('Sensor output').getId();
    prId=PropertyIdentifier.presentValue; 
    result=communicate.communicateRead(ssm,sensorAddress,bacnetNetworkPort,objId, prId).floatValue();
    if strcmp(sensorAnnotation.getUnits(),'celsius')
        value=result;
    elseif strcmp(sensorAnnotation.getUnits(),'fahrenheit')
        value=SEMfunction1(result);
    end
    loc=where(sensorAnnotation.getLocation());
    when=sensorAnnotation.getLastMeasurementTime();
    accur=sensorAnnotation.getAccuracy();
    
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wt=weightTime(currentTime, when)
    rejectTime=5/60;
    if (currentTime-when > rejectTime)
        wt=0;
    else
        wt=((rejectTime-(currentTime-when)))^2;
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wa=weightAccuracy(accur)
    wa=1/accur^2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [K L] = findDesignParametersController(A,B,Poles)    
    %A-B*K+Poles=0;
    Binv=(B'*B)*B';
    K=B\(A-Poles);
    L=Binv;          
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
